#include <machine/asmdefs.h>

ASM_HEADER

FNDEFN_BEGIN(derotation_asm)

        /* load inI, inQ */
        lr              %vr0,%r8,0          ||          sli         %r12,%r12,1     # multiply phase update by 2
        lr              %vr1,%r8,32         ||          addil       %r14,%r10,1026  # put sin table ptr in r14
        lr              %vr2,%r8,64         ||          ctsr        %r13,5
        lr              %vr3,%r8,96         ||          lirl        %r8,512

        /* downsample inI, inQ */
        rshf1           %vr1,%vr0,7         ||          stw         %r0,%r1,-4      ||          sli         %r8,%r8,12     # put 0x200000 in r8
        rshf1           %vr3,%vr2,7         ||          stwul       %r1,%r1,-128
        addi            %r3,%r1,4

LOOP_DEROTATE:
        /* add counter to 0x200000   |   store cos from previous iter (first store is a dummy store) */
        add             %r0,%r11,%r8        ||          stuhu       %r13,%r3,2
        /* extract bits 30-21 (shift right by 21 plus mask) to create index */
        extri           %r0,%r0,30,21
        /* zero out LSB of index     |   store sin from previous iter (first store is a dummy store) */
        andi            %r0,%r0,-2          ||          stuhl       %r15,%r3,32
        /* add index to cos table */
        add             %r13,%r10,%r0
        /* add index to sin table    |   load val from cos table */
        add             %r15,%r14,%r0       ||          luh         %r13,%r13,0
        /* inc counter by phaseupd   |   load val from sin table     |       loop */
        add             %r11,%r11,%r12      ||          luh         %r15,%r15,0   ||  loop            0,%lc1,LOOP_DEROTATE

        /* final stores of cos, sin, then load as a vector   |   set up vector of 1s in vr1 */
        stuhu           %r13,%r3,2          ||          lir         %r8,2
        stuhl           %r15,%r3,32         ||          sli         %r8,%r8,15
        addil           %r3,%r1,8
        lr              %vr4,%r3,0          ||          ctsr        %r8,73
        lr              %vr5,%r3,32         ||          rbroad      %vr1,%ac1,0

        /* multiply inI, inQ by cos, sin vectors     |   reset stack */
        rmuls           %vr6,%vr0,%vr4
        rmuls           %vr7,%vr2,%vr5
        radds           %vr7,%vr6,%vr7
        rsra            %vr7,%vr7,%vr1

        rmuls           %vr6,%vr2,%vr4      ||          str         %vr7,%r9,0
        rmuls           %vr7,%vr0,%vr5      ||      	lw          %r1,%r1,0
        rsubfs          %vr7,%vr7,%vr6      ||          addi        %r8,%r11,0
        rsra            %vr7,%vr7,%vr1      ||          lw          %r0,%r1,-4
        str             %vr7,%r9,64         ||          ji	    %jt0,8

FNDEFN_END(derotation_asm)

